#include <reactive_behavior/test/reactive_behavior_test.h>
#include <iostream>

using std::cout;
using std::cin;
using std::endl;

using PlayerCc::dtor;

static const double kPi = 3.14159265;

// Note: this test is for stage simulation only.
// TODO request initial pose from user to enable real testing. 
void TESTReactiveBehavior() {
  // Set up player's objects.
  PlayerCc::PlayerClient player_client("localhost");
  PlayerCc::Position2dProxy position2d_proxy(&player_client);
  PlayerCc::SonarProxy sonar_proxy(&player_client);

  position2d_proxy.SetMotorEnable(1);
  position2d_proxy.RequestGeom();
  sonar_proxy.RequestGeom();
  player_client.Read();
  position2d_proxy.SetOdometry(-4.5, -3.0, kPi / 4);
  player_client.Read();

  // Create synchronized access current pose object.
  SyncedCurrentPose synced_current_pose;

  // Create reactive behavior object.
  ReactiveBehavior reactive_behavior(&player_client, 
                                     &position2d_proxy, 
                                     &sonar_proxy, 
                                     &synced_current_pose);

  // Create localization estimator and spawn associated thread.
  FakeLocalizationEstimator localization_estimator(&player_client, &position2d_proxy, &synced_current_pose);
  boost::thread localization_estimator_thread(
      boost::ref(localization_estimator));

  // Let user choose which test to run.
  cout << "Enter a suboption:" << endl;
  cout << "z: Move to (-3.5, -2.0, kPi / 4). No obstacles. [CAVE]" << endl;
  cout << "a: Move to (3.0, -7.0, kPi / 2). No obstacles. [CAVE]" << endl;
  cout << "b: Move to (5.0, -2.0, -kPi / 2). One obstacle. [CAVE]" << endl;
  cout << "c: Move to (7.0, 7.0, kPi). Many obstacles. [CAVE]" << endl;
  cout << "d: Move to (-3.0, 0.0, 0.0). No obstacles. Narrow door. [SIMPLE ROOMS]" << endl;
  cout << "e: Move to (4.0, 3.0, 0.0). Many obstacles. Narrow doors. [SIMPLE ROOMS]" << endl;
  cout << "f: Move to (4.0, -3.0, kPi). Many obstacles. Narrow doors. [SIMPLE ROOMS]" << endl;
  cout << ">>> ";
  char option;
  cin >> option;
  player_point_2d goal;
  double goal_yaw;
  switch (option) {
    case 'z':
      goal.px = -3.5;
      goal.py = -2.0;
      goal_yaw = kPi / 4;
    case 'a':
      goal.px = 3.0;
      goal.py = -7.0;
      goal_yaw = kPi / 2;
      break;
    case 'b':
      goal.px = 5.0;
      goal.py = -2.0;
      goal_yaw = -kPi / 2;
      break;
    case 'c':
      goal.px = 7.0;
      goal.py = 7.0;
      goal_yaw = kPi;
      break;
    case 'd':
      goal.px = -3.0;
      goal.py = 0.0;
      goal_yaw = 0.0;
      break;
    case 'e':
      goal.px = 4.0;
      goal.py = 3.0;
      goal_yaw = 0.0;
      break;
    case 'f':
      goal.px = 4.0;
      goal.py = -3.0;
      goal_yaw = kPi;
      break;
    default:
      cout << "Invalid option. Ending execution." << endl;
  }

  // Try to reach goal by using reactive behavior only.
  bool success = reactive_behavior.MoveToGoalAvoidingObstacles(goal);
  reactive_behavior.RotateTo(goal_yaw);

  localization_estimator_thread.join();
  // Note: program never ends.
}
